#ifndef C_CAMERA_H
#define C_CAMERA_H


#include "cvxCapture.h"

class CCamera
{
public:
	CCamera();	
	~CCamera();

	//capture images from video or camera
	bool calib(int flags, bool computerErr);
	void computerParameterError(void);
	void computerEulerAngle(float unit_length, int w, int h);
	void computerNormal(float unit_length, const CvSize &patternSize);

	void bundleAdjust3DLinear(float unitLength, const CvSize &patternSize);
//	void boudleAdjustNonLinear(float unitLength, const CvSize &patternSize);

	void saveResult(const string& filename);
	void saveCamera(const string& filename);
	void saveIntrinsic(const string &filename);
	void saveExtrinsic(const string &filename);
	void saveSphereEulerAngle(const string &filename, float unit_length, int w, int h);
	//visualize reprojection error, 
	void visualizeRPR(const CvSize &patternSize, int magFactor);


	void saveIntrinsic(const string &filename, bool isAppend);

	void loadCamera(const string& filename);
	void saveText(const string &videoFileName, const string &fielName);
	void printReprojectionError(void);

	void storeCurrentCorner2d(void);
	void setCorners3d(const vector<Point3f>& pts3d);

	void cleanCorners(void){m_objectPoints.clear(); m_imagePoints.clear();}

	unsigned int getFeatureNumber(void){return m_imagePoints.size();}

	void setFeature3d(const vector<vector<Point3f> > &pts) {m_objectPoints = pts;}
	void setFeature2d(const vector<vector<Point2f> > &pts) {m_imagePoints.clear(); m_imagePoints = pts;}	
	void setFlags(int flag);
	void setImageSize(const CvSize &sz){m_imageSize = sz;}
	void addFeature2d(const vector<Point2f> &pts) {m_imagePoints.push_back(pts);}
	void addFeature2dAnd3d(const vector<Point2f> &pts, const vector<Point3f> &pts3d);
	

	const vector<vector<Point3f> > & getFeaturePoints3d(void){return m_objectPoints;}
	const vector<vector<Point2f> > & getFeaturePoints2d(void){return m_imagePoints;}
	Mat & getIntrinsicMatrix(void) {return m_intrinsic;}
	Mat & getDistortionCoeffs(void) {return m_distortionCoeffs;}
	vector<float> & getReprojectionError(void){return m_reprojErrs;}
	void getRotationVec(vector<Mat> &rvecs){rvecs = m_RM;}
	void getTranslationVec(vector<Mat> &tvecs){tvecs = m_TM;}

	//write intrinsic matrix and coeff to a buf
	void GetResult(char *buf);
	//write intrinsic matrix and coeff to a buf
	void GetIntrinsicParameter(char *buf);
	//get the orientation of another camera with the same reference points
	void GetOrientation(CCamera &otherCamera, bool useEightPts, char *buf);

	CvSize getImageSize(void){return m_imageSize;}


	//computer reprojection error of a pair of image
	static double GetReProjectionError(const Mat& cameraMatrix, const Mat& distCoeffs, 									
										 const vector<Point3f> &objectPoints,
										 const vector<Point2f>& imagePoints, 
										 FILE *pf);

public:
	vector<float> m_thetaVec;
	vector<float> m_phiVec;
	vector<float> m_zDistanceVec;
	vector<Vec3d> m_eulerAngleVec;
	vector<float> m_rodriguesAngle;
	vector<Vec3d> m_planeNormal;

private:
	Mat m_currentFrame;      //
	CvSize m_imageSize;   //image resolution
	vector<Mat> m_frames;	
	

	vector<Point2f> m_currentCorners2d;  //current frame corner position in image

	vector<vector<Point3f> > m_objectPoints;     //all frames objects position
	vector<vector<Point2f> > m_imagePoints;      //all frames image corner position
	vector<vector<Point2f> > m_reprojectPts;     //all frame re-projection points position


	//camera parameters
	//3*3
	Mat m_intrinsic;
	Mat m_intrisicGuess;
	//4*1 or 5*1 or 8*1
	Mat m_distortionCoeffs;
	double	m_aspectRatio;   //for canon xf105 is near 1.0
	Mat m_parameterError;    //focal length 2, principle point 2, distortion 4;
	Mat m_stdDev;            //standard deviation of reprojection error

	//physical 
	double m_apertureWidth;        //1/3" type 4.8mm
	double m_apertureHeight;       //          3.6mm
	double m_focalLength;          //unit mm
	Point2d m_principalPoint;      //unit pixel
	double m_fovx;  //field of view in degrees along the horizontal sensor axis
	double m_fovy;  //field of view in degrees along the vertical sensor axis

	//extrinsic vectors
	//3*1
	vector<Mat> m_RM;	

	//3*1
	vector<Mat> m_TM;

	int m_flags;	
	vector<float> m_reprojErrs;
	double m_totalAvgErr;


private:
	
	void initCamera();

	//get chessboard points from an image
	bool getChessboardPoints(const CvSize &boardSize);
	

	//
	void configFlags();


	bool runCalibration(bool computerErr);
	
	
private:

	static void drawChessboardCorners( Mat& image, const vector<Point2f> & corners);

	//static calibration interface 
	static bool calibration( const vector<vector<cv::Point3f> > &objectPoints, 
							 const vector<vector<cv::Point2f> > &imagePoints,
							 CvSize imageSize, int flags, Mat& cameraMatrix, Mat& distCoeffs,
							 vector<Mat>& rvecs, vector<Mat>& tvecs);

	

	/************************************************************************/
	/* 
		perViewError:average error of per image
	*/
	/************************************************************************/
	static  double computeReprojectionErrors(const vector<vector<Point3f> >& objectPoints,
											 const vector<vector<Point2f> >& imagePoints,
											 const vector<Mat>& rvecs, const vector<Mat>& tvecs,
											 const Mat& cameraMatrix, const Mat& distCoeffs,
											 vector<float>& perViewErrors,  
											 vector<vector<Point2f> >& reprojPts);

	static void saveCameraParams( const string& filename, Size imageSize, int flags,
								  const Mat& cameraMatrix, const Mat& distCoeffs,
								  const vector<Mat>& rvecs, const vector<Mat>& tvecs,
								  const vector<float>& reprojErrs,
								  const vector<vector<Point2f> >& imagePoints, double totalAvgErr ,
								  const Vec2d &principalPoint, double focalLength, double aspectRatio);



};


#endif